/*********************************************************************
* Author: CJH
*********************************************************************/
#ifndef __BSPLINES_PLANNER_H_
#define __BSPLINES_PLANNER_H_
#include <ros/ros.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h>
#include <nav_core/base_global_planner.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>


#include <base_local_planner/world_model.h>
#include <base_local_planner/costmap_model.h>

#include <algorithm>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>

#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>



#include<iostream>
#include<thread>
#include<vector>
#include<list>
#include <random>
#include<ctime>




//设置类外回调函数
namespace bsplines_planner{

  class Bsplines_planner : public nav_core::BaseGlobalPlanner {

    //放置构造函数与解析函数
    
    public:
      Bsplines_planner();

      ~Bsplines_planner();

    public:

      Bsplines_planner(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
      void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
      /*Used to return to a planned route*/
      void routeFitting(const geometry_msgs::PoseStamped& start, 
      const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& path);
      /*Set up a function that listens and accepts and publishes waypoints*/
      void waypointCallback(const geometry_msgs::PointStampedConstPtr& waypoint);
      void pointListener();
      void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path);
      bool makePlan(const geometry_msgs::PoseStamped& start, 
          const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan);

    private:
      double radius;//用于表示车底盘面积，从而计算经过路径
      double stride;//用于拟合的步长
      bool initialized_;//用于确认是否初始化
      std::string name_;//用于名空间

      

    /*路径点的私类函数接口*/
    private:
      typedef Eigen::Vector2d Point2f; //二维平面点
      void Markerinit(const int action,const int type,const float scale);//初始化MarkerArray的一些参数
      void MarkerArrayprint(const int action,const int type);//用于在地图上显示MarkerArray
	    void waypoint_add(const geometry_msgs::PointStampedConstPtr &newpoint);
      void waypoint_delete();//用于删除已经经过的路径点
      void waypoint_clear();//一次性删除所有点

    /*用于多点巡航cruise接口*/
    private:
        bool doing_;
        geometry_msgs::PoseStamped start_;
        boost::mutex mutex_;
         /*move_base导航服务器声明*/
        typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;


    /*ros节点的私类变量*/
    private:
      ros::NodeHandle nh;
      //声明订阅节点与发布节点
      ros::Subscriber Sub_;
      ros::Publisher Pub_;
      ros::Publisher Pub_path_;
      //声明MarkerArray，作为可视化点
      visualization_msgs::Marker costArray;
      visualization_msgs::MarkerArray costArrays;
      
      

    /*用于监听路径点的保护对象*/
    protected:
      std::list<geometry_msgs::PointStampedConstPtr> Waypoints_list;  //导航路径点的双向队列
  };
};  
#endif
